\doxysection{kalman\+\_\+filter.\+h}
\hypertarget{kalman__filter_8h_source}{}\label{kalman__filter_8h_source}\index{C:/Users/ASUS/Desktop/dm-\/ctrlH7-\/balance-\/9025test/modules/algorithm/kalman\_filter.h@{C:/Users/ASUS/Desktop/dm-\/ctrlH7-\/balance-\/9025test/modules/algorithm/kalman\_filter.h}}

\begin{DoxyCode}{0}
\DoxyCodeLine{00001\ }
\DoxyCodeLine{00013\ \textcolor{preprocessor}{\#ifndef\ \_\_KALMAN\_FILTER\_H}}
\DoxyCodeLine{00014\ \textcolor{preprocessor}{\#define\ \_\_KALMAN\_FILTER\_H}}
\DoxyCodeLine{00015\ }
\DoxyCodeLine{00016\ \textcolor{comment}{//\ cortex-\/m4\ DSP\ lib}}
\DoxyCodeLine{00017\ \textcolor{comment}{/*}}
\DoxyCodeLine{00018\ \textcolor{comment}{\#define\ \_\_CC\_ARM\ \ \ \ //\ Keil}}
\DoxyCodeLine{00019\ \textcolor{comment}{\#define\ ARM\_MATH\_CM4}}
\DoxyCodeLine{00020\ \textcolor{comment}{\#define\ ARM\_MATH\_MATRIX\_CHECK}}
\DoxyCodeLine{00021\ \textcolor{comment}{\#define\ ARM\_MATH\_ROUNDING}}
\DoxyCodeLine{00022\ \textcolor{comment}{\#define\ ARM\_MATH\_DSP\ \ \ \ //\ define\ in\ arm\_math.h}}
\DoxyCodeLine{00023\ \textcolor{comment}{*/}}
\DoxyCodeLine{00024\ }
\DoxyCodeLine{00025\ \textcolor{preprocessor}{\#include\ "{}\mbox{\hyperlink{stm32h723xx_8h}{stm32h723xx.h}}"{}}}
\DoxyCodeLine{00026\ \textcolor{preprocessor}{\#include\ "{}arm\_math.h"{}}}
\DoxyCodeLine{00027\ \textcolor{comment}{//\#include\ "{}dsp/matrix\_functions.h"{}}}
\DoxyCodeLine{00028\ \textcolor{preprocessor}{\#include\ "{}math.h"{}}}
\DoxyCodeLine{00029\ \textcolor{preprocessor}{\#include\ "{}stdint.h"{}}}
\DoxyCodeLine{00030\ \textcolor{preprocessor}{\#include\ "{}stdlib.h"{}}}
\DoxyCodeLine{00031\ }
\DoxyCodeLine{00032\ \textcolor{preprocessor}{\#ifndef\ user\_malloc}}
\DoxyCodeLine{00033\ \textcolor{preprocessor}{\#ifdef\ \_CMSIS\_OS\_H}}
\DoxyCodeLine{00034\ \textcolor{preprocessor}{\#define\ user\_malloc\ pvPortMalloc}}
\DoxyCodeLine{00035\ \textcolor{preprocessor}{\#else}}
\DoxyCodeLine{00036\ \textcolor{preprocessor}{\#define\ user\_malloc\ malloc}}
\DoxyCodeLine{00037\ \textcolor{preprocessor}{\#endif}}
\DoxyCodeLine{00038\ \textcolor{preprocessor}{\#endif}}
\DoxyCodeLine{00039\ }
\DoxyCodeLine{00040\ \textcolor{comment}{//\ 若运算速度不够,可以使用q31代替f32,但是精度会降低}}
\DoxyCodeLine{00041\ \textcolor{preprocessor}{\#define\ mat\ arm\_matrix\_instance\_f32}}
\DoxyCodeLine{00042\ \textcolor{preprocessor}{\#define\ Matrix\_Init\ arm\_mat\_init\_f32}}
\DoxyCodeLine{00043\ \textcolor{preprocessor}{\#define\ Matrix\_Add\ arm\_mat\_add\_f32}}
\DoxyCodeLine{00044\ \textcolor{preprocessor}{\#define\ Matrix\_Subtract\ arm\_mat\_sub\_f32}}
\DoxyCodeLine{00045\ \textcolor{preprocessor}{\#define\ Matrix\_Multiply\ arm\_mat\_mult\_f32}}
\DoxyCodeLine{00046\ \textcolor{preprocessor}{\#define\ Matrix\_Transpose\ arm\_mat\_trans\_f32}}
\DoxyCodeLine{00047\ \textcolor{preprocessor}{\#define\ Matrix\_Inverse\ arm\_mat\_inverse\_f32}}
\DoxyCodeLine{00048\ }
\DoxyCodeLine{00049\ \textcolor{keyword}{typedef}\ \textcolor{keyword}{struct\ }\mbox{\hyperlink{structkf__t}{kf\_t}}}
\DoxyCodeLine{00050\ \{}
\DoxyCodeLine{00051\ \ \ \ \ \textcolor{keywordtype}{float}\ *FilteredValue;}
\DoxyCodeLine{00052\ \ \ \ \ \textcolor{keywordtype}{float}\ *MeasuredVector;}
\DoxyCodeLine{00053\ \ \ \ \ \textcolor{keywordtype}{float}\ *ControlVector;}
\DoxyCodeLine{00054\ }
\DoxyCodeLine{00055\ \ \ \ \ uint8\_t\ xhatSize;}
\DoxyCodeLine{00056\ \ \ \ \ uint8\_t\ uSize;}
\DoxyCodeLine{00057\ \ \ \ \ uint8\_t\ zSize;}
\DoxyCodeLine{00058\ }
\DoxyCodeLine{00059\ \ \ \ \ uint8\_t\ UseAutoAdjustment;}
\DoxyCodeLine{00060\ \ \ \ \ uint8\_t\ MeasurementValidNum;}
\DoxyCodeLine{00061\ }
\DoxyCodeLine{00062\ \ \ \ \ uint8\_t\ *MeasurementMap;\ \ \ \ \ \ \textcolor{comment}{//\ 量测与状态的关系\ how\ measurement\ relates\ to\ the\ state}}
\DoxyCodeLine{00063\ \ \ \ \ \textcolor{keywordtype}{float}\ *MeasurementDegree;\ \ \ \ \ \textcolor{comment}{//\ 测量值对应H矩阵元素值\ elements\ of\ each\ measurement\ in\ H}}
\DoxyCodeLine{00064\ \ \ \ \ \textcolor{keywordtype}{float}\ *MatR\_DiagonalElements;\ \textcolor{comment}{//\ 量测方差\ variance\ for\ each\ measurement}}
\DoxyCodeLine{00065\ \ \ \ \ \textcolor{keywordtype}{float}\ *StateMinVariance;\ \ \ \ \ \ \textcolor{comment}{//\ 最小方差\ 避免方差过度收敛\ suppress\ filter\ excessive\ convergence}}
\DoxyCodeLine{00066\ \ \ \ \ uint8\_t\ *temp;}
\DoxyCodeLine{00067\ }
\DoxyCodeLine{00068\ \ \ \ \ \textcolor{comment}{//\ 配合用户定义函数使用,作为标志位用于判断是否要跳过标准KF中五个环节中的任意一个}}
\DoxyCodeLine{00069\ \ \ \ \ uint8\_t\ SkipEq1,\ SkipEq2,\ SkipEq3,\ SkipEq4,\ SkipEq5;}
\DoxyCodeLine{00070\ }
\DoxyCodeLine{00071\ \ \ \ \ \textcolor{comment}{//\ definiion\ of\ struct\ mat:\ rows\ \&\ cols\ \&\ pointer\ to\ vars}}
\DoxyCodeLine{00072\ \ \ \ \ mat\ xhat;\ \ \ \ \ \ \textcolor{comment}{//\ x(k|k)}}
\DoxyCodeLine{00073\ \ \ \ \ mat\ xhatminus;\ \textcolor{comment}{//\ x(k|k-\/1)}}
\DoxyCodeLine{00074\ \ \ \ \ mat\ u;\ \ \ \ \ \ \ \ \ \textcolor{comment}{//\ control\ vector\ u}}
\DoxyCodeLine{00075\ \ \ \ \ mat\ z;\ \ \ \ \ \ \ \ \ \textcolor{comment}{//\ measurement\ vector\ z}}
\DoxyCodeLine{00076\ \ \ \ \ mat\ P;\ \ \ \ \ \ \ \ \ \textcolor{comment}{//\ covariance\ matrix\ P(k|k)}}
\DoxyCodeLine{00077\ \ \ \ \ mat\ Pminus;\ \ \ \ \textcolor{comment}{//\ covariance\ matrix\ P(k|k-\/1)}}
\DoxyCodeLine{00078\ \ \ \ \ mat\ F,\ FT;\ \ \ \ \ \textcolor{comment}{//\ state\ transition\ matrix\ F\ FT}}
\DoxyCodeLine{00079\ \ \ \ \ mat\ B;\ \ \ \ \ \ \ \ \ \textcolor{comment}{//\ control\ matrix\ B}}
\DoxyCodeLine{00080\ \ \ \ \ mat\ H,\ HT;\ \ \ \ \ \textcolor{comment}{//\ measurement\ matrix\ H}}
\DoxyCodeLine{00081\ \ \ \ \ mat\ Q;\ \ \ \ \ \ \ \ \ \textcolor{comment}{//\ process\ noise\ covariance\ matrix\ Q}}
\DoxyCodeLine{00082\ \ \ \ \ mat\ R;\ \ \ \ \ \ \ \ \ \textcolor{comment}{//\ measurement\ noise\ covariance\ matrix\ R}}
\DoxyCodeLine{00083\ \ \ \ \ mat\ K;\ \ \ \ \ \ \ \ \ \textcolor{comment}{//\ kalman\ gain\ \ K}}
\DoxyCodeLine{00084\ \ \ \ \ mat\ S,\ temp\_matrix,\ temp\_matrix1,\ temp\_vector,\ temp\_vector1;}
\DoxyCodeLine{00085\ }
\DoxyCodeLine{00086\ \ \ \ \ int8\_t\ MatStatus;}
\DoxyCodeLine{00087\ }
\DoxyCodeLine{00088\ \ \ \ \ \textcolor{comment}{//\ 用户定义函数,可以替换或扩展基准KF的功能}}
\DoxyCodeLine{00089\ \ \ \ \ void\ (*User\_Func0\_f)(\textcolor{keyword}{struct\ }\mbox{\hyperlink{structkf__t}{kf\_t}}\ *kf);}
\DoxyCodeLine{00090\ \ \ \ \ void\ (*User\_Func1\_f)(\textcolor{keyword}{struct\ }\mbox{\hyperlink{structkf__t}{kf\_t}}\ *kf);}
\DoxyCodeLine{00091\ \ \ \ \ void\ (*User\_Func2\_f)(\textcolor{keyword}{struct\ }\mbox{\hyperlink{structkf__t}{kf\_t}}\ *kf);}
\DoxyCodeLine{00092\ \ \ \ \ void\ (*User\_Func3\_f)(\textcolor{keyword}{struct\ }\mbox{\hyperlink{structkf__t}{kf\_t}}\ *kf);}
\DoxyCodeLine{00093\ \ \ \ \ void\ (*User\_Func4\_f)(\textcolor{keyword}{struct\ }\mbox{\hyperlink{structkf__t}{kf\_t}}\ *kf);}
\DoxyCodeLine{00094\ \ \ \ \ void\ (*User\_Func5\_f)(\textcolor{keyword}{struct\ }\mbox{\hyperlink{structkf__t}{kf\_t}}\ *kf);}
\DoxyCodeLine{00095\ \ \ \ \ void\ (*User\_Func6\_f)(\textcolor{keyword}{struct\ }\mbox{\hyperlink{structkf__t}{kf\_t}}\ *kf);}
\DoxyCodeLine{00096\ }
\DoxyCodeLine{00097\ \ \ \ \ \textcolor{comment}{//\ 矩阵存储空间指针}}
\DoxyCodeLine{00098\ \ \ \ \ \textcolor{keywordtype}{float}\ *xhat\_data,\ *xhatminus\_data;}
\DoxyCodeLine{00099\ \ \ \ \ \textcolor{keywordtype}{float}\ *u\_data;}
\DoxyCodeLine{00100\ \ \ \ \ \textcolor{keywordtype}{float}\ *z\_data;}
\DoxyCodeLine{00101\ \ \ \ \ \textcolor{keywordtype}{float}\ *P\_data,\ *Pminus\_data;}
\DoxyCodeLine{00102\ \ \ \ \ \textcolor{keywordtype}{float}\ *F\_data,\ *FT\_data;}
\DoxyCodeLine{00103\ \ \ \ \ \textcolor{keywordtype}{float}\ *B\_data;}
\DoxyCodeLine{00104\ \ \ \ \ \textcolor{keywordtype}{float}\ *H\_data,\ *HT\_data;}
\DoxyCodeLine{00105\ \ \ \ \ \textcolor{keywordtype}{float}\ *Q\_data;}
\DoxyCodeLine{00106\ \ \ \ \ \textcolor{keywordtype}{float}\ *R\_data;}
\DoxyCodeLine{00107\ \ \ \ \ \textcolor{keywordtype}{float}\ *K\_data;}
\DoxyCodeLine{00108\ \ \ \ \ \textcolor{keywordtype}{float}\ *S\_data,\ *temp\_matrix\_data,\ *temp\_matrix\_data1,\ *temp\_vector\_data,\ *temp\_vector\_data1;}
\DoxyCodeLine{00109\ \}\ KalmanFilter\_t;}
\DoxyCodeLine{00110\ }
\DoxyCodeLine{00111\ \textcolor{keyword}{extern}\ uint16\_t\ sizeof\_float,\ sizeof\_double;}
\DoxyCodeLine{00112\ }
\DoxyCodeLine{00113\ \textcolor{keywordtype}{void}\ Kalman\_Filter\_Init(KalmanFilter\_t\ *kf,\ uint8\_t\ xhatSize,\ uint8\_t\ uSize,\ uint8\_t\ zSize);}
\DoxyCodeLine{00114\ \textcolor{keywordtype}{void}\ Kalman\_Filter\_Measure(KalmanFilter\_t\ *kf);}
\DoxyCodeLine{00115\ \textcolor{keywordtype}{void}\ Kalman\_Filter\_xhatMinusUpdate(KalmanFilter\_t\ *kf);}
\DoxyCodeLine{00116\ \textcolor{keywordtype}{void}\ Kalman\_Filter\_PminusUpdate(KalmanFilter\_t\ *kf);}
\DoxyCodeLine{00117\ \textcolor{keywordtype}{void}\ Kalman\_Filter\_SetK(KalmanFilter\_t\ *kf);}
\DoxyCodeLine{00118\ \textcolor{keywordtype}{void}\ Kalman\_Filter\_xhatUpdate(KalmanFilter\_t\ *kf);}
\DoxyCodeLine{00119\ \textcolor{keywordtype}{void}\ Kalman\_Filter\_P\_Update(KalmanFilter\_t\ *kf);}
\DoxyCodeLine{00120\ \textcolor{keywordtype}{float}\ *Kalman\_Filter\_Update(KalmanFilter\_t\ *kf);}
\DoxyCodeLine{00121\ }
\DoxyCodeLine{00122\ \textcolor{preprocessor}{\#endif\ }\textcolor{comment}{//\_\_KALMAN\_FILTER\_H}}

\end{DoxyCode}
